#include "mpc_controller.h"
#include "my_functions.h"

size_t N = 20;  // how many timesteps into the future
double dt = 0.01; // duration

const double Lf = 1.0; //车辆前轴到车辆质心的距离

// cost function中的期望值
double ref_cte = 0;     //横向偏差期望值[m]
double ref_ephi = 0;    //航向偏差期望值[rad]
double ref_v = 60;      //期望速度[m/s]

// 为了增加代码可读性，给vars向量的索引命名
size_t x_start = 0;
size_t y_start = x_start + N;
size_t phi_start = y_start + N;
size_t v_start = phi_start + N;
size_t cte_start = v_start + N;
size_t ephi_start = cte_start + N;
size_t delta_start = ephi_start + N;
size_t a_start = delta_start + N - 1;

// MPC类的实现.

MPC::MPC() {}

MPC::~MPC() {}

vector<double> MPC::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs) {

    bool ok = true;
    typedef CPPAD_TESTVECTOR(double) Dvector;
    /**1设置好变量vars，便于ipopt求解器求解vars = [X,Y,PHI,V,CTE,EPHI,DELTA,A]
     * 其中X,Y,PHI,V,CTE,EPHI有N个元素，DELTA,A有N-1个元素
     * ***************************************************/

    // 1.1将车辆状态state向量里的物理量分解出来，提高代码可读性
    double x = state[0];
    double y = state[1];
    double phi = state[2];
    double v = state[3];
    double cte = state[4];
    double ephi = state[5];
    // 1.2计算变量vars的个数（包括所有的车辆状态和车辆控制输入）
    size_t n_vars = N * 6 + (N - 1) * 2;
    // 1.3将所有的模型变量初始化为0.
    Dvector vars(n_vars);
    for (size_t i = 0; i < n_vars; i++) {
        vars[i] = 0.0;
    }
    // 1.4限制vars中各个元素的范围
    // 1.4.1状态变量states[X,Y,PHI,V,CTE,EPHI]的范围
    Dvector vars_lower_bounds(n_vars);
    Dvector vars_upper_bounds(n_vars);
    for (size_t i = 0; i < delta_start; ++i) {
        vars_lower_bounds[i] = -1.0e19;
        vars_upper_bounds[i] = 1.0e19;
    }
    // 1.4.2为车轮转角DELTA设置上下限
    double max_degree = 35;         //限定最大转角[deg].
    double max_radians = myNumpy::deg2rad(max_degree);
    for (size_t i = delta_start; i < a_start; ++i) {
        vars_lower_bounds[i] = -max_radians;
        vars_upper_bounds[i] = +max_radians;
    }

    // 1.4.3为加速度A设置上下限
    double max_acceleration_value = 1.0;    //最大加速度[m/s^2]
    for (size_t i = a_start; i < n_vars; ++i) {
        vars_lower_bounds[i] = -max_acceleration_value;
        vars_upper_bounds[i] = +max_acceleration_value;
    }
    /**************************2 设置好车辆模型的约束，便于ipopt求解器求解******************************/
    // 2.1设置约束变量的个数
    size_t n_constraints = N * 6;
    // 2.2限定约束变量的范围（上下限）
    Dvector constraints_lower_bounds(n_constraints);
    Dvector constraints_upper_bounds(n_constraints);
    for (size_t i = 0; i < n_constraints; ++i) {
        constraints_lower_bounds[i] = 0;
        constraints_upper_bounds[i] = 0;
    }

    // 让求解器从当前状态空间开始求解Force the solver to start from current state in optimization space
    constraints_lower_bounds[x_start] = x;
    constraints_upper_bounds[x_start] = x;
    constraints_lower_bounds[y_start] = y;
    constraints_upper_bounds[y_start] = y;
    constraints_lower_bounds[phi_start] = phi;
    constraints_upper_bounds[phi_start] = phi;
    constraints_lower_bounds[v_start] = v;
    constraints_upper_bounds[v_start] = v;
    constraints_lower_bounds[cte_start] = cte;
    constraints_upper_bounds[cte_start] = cte;
    constraints_lower_bounds[ephi_start] = ephi;
    constraints_upper_bounds[ephi_start] = ephi;

    // new 一个FG_eval实例，用来计算模型的约束方程和cost function
    FG_eval fg_eval(std::move(coeffs));
    // 针对ipopt求解器的设置
    //
    // NOTE: You don't have to worry about these options
    //
    // options for IPOPT solver
    std::string options;
    // Uncomment this if you'd like more print information
    options += "Integer print_level  0\n";
    // NOTE: Setting sparse to true allows the solver to take advantage
    // of sparse routines, this makes the computation MUCH FASTER. If you
    // can uncomment 1 of these and see if it makes a difference or not but
    // if you uncomment both the computation time should go up in orders of
    // magnitude.
    options += "Sparse  true        forward\n";
    options += "Sparse  true        reverse\n";
    // NOTE: Currently the solver has a maximum time limit of 0.5 seconds.
    // Change this as you see fit.
    options += "Numeric max_cpu_time          0.5\n";

    // place to return solution
    CppAD::ipopt::solve_result<Dvector> solution;

    // solve the problem
    CppAD::ipopt::solve<Dvector, FG_eval>(
            options, vars, vars_lower_bounds, vars_upper_bounds, constraints_lower_bounds,
            constraints_upper_bounds, fg_eval, solution);

    // Check some of the solution values
    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;

    // 打印cost function的值
    auto cost = solution.obj_value;
    std::cout << "Cost " << cost << std::endl;

    // 返回ipopt求解器计算的向量result = [x, y, phi, v, cte, ephi, delta, a]
    vector<double> result;
    result.push_back(solution.x[x_start + 1]);              //索引0，mpc预测的x坐标
    result.push_back(solution.x[y_start + 1]);
    result.push_back(solution.x[phi_start + 1]);
    result.push_back(solution.x[v_start + 1]);
    result.push_back(solution.x[cte_start + 1]);            //索引4横向偏差
    result.push_back(solution.x[ephi_start + 1]);
    result.push_back(solution.x[delta_start]);              //索引6，mpc预测的车轮转角[deg]
    result.push_back(solution.x[a_start]);

    return result;
}

// FG_eval类的实现
FG_eval::FG_eval(Eigen::VectorXd coeffs) {
    this->coeffs_ = std::move(coeffs);
}

void FG_eval::operator()(FG_eval::ADvector &fg, const FG_eval::ADvector &vars) {
    /******************1定义cost function，并且把cost value储存到fg[0]中**************************/
    fg[0] = 0;      //fg[0]储存cost value的值，因此此处的约束向量fg比起MPC::Solve中的约束向量多出一个元素

    // 1.1给cost function函数的各项设置权重(Define weights for different terms of objective)
    const double cte_weight = 2000;
    const double ephi_weight = 2000;
    const double v_weight = 100;
    const double actuator_cost_weight = 10;
    const double change_steer_cost_weight = 100000;
    const double change_accel_cost_weight = 10000;

    // 1.2.1,cost function优化目标1：使各项接近期望值(Objective term 1: Keep close to reference values)
    for (size_t t = 0; t < N; ++t) {
        fg[0] += cte_weight * CppAD::pow(vars[cte_start + t] - ref_cte, 2);
        fg[0] += ephi_weight * CppAD::pow(vars[ephi_start + t] - ref_ephi, 2);
        fg[0] += v_weight * CppAD::pow(vars[v_start + t] - ref_v, 2);
    }

    // 1.2.2,cost function优化目标2,避免方向盘和油门的频繁使用(Avoid to actuate, as much as possible)
    for (size_t t = 0; t < N - 1; ++t) {
        fg[0] += actuator_cost_weight * CppAD::pow(vars[delta_start + t], 2);
        fg[0] += actuator_cost_weight * CppAD::pow(vars[a_start + t], 2);
    }

    // 1.2.3，cost function优化目标3，让执行器的变化更加的平滑:  Enforce actuators smoothness in change
    for (size_t t = 0; t < N - 2; ++t) {
        fg[0] += change_steer_cost_weight * CppAD::pow(vars[delta_start + t + 1] - vars[delta_start + t], 2);
        fg[0] += change_accel_cost_weight * CppAD::pow(vars[a_start + t + 1] - vars[a_start + t], 2);
    }
    /******************2向量fg的初始位置约束和车辆动力学模型的约束**************************/
    // 2.1fg的初始位置约束
    fg[1 + x_start] = vars[x_start];        //因为fg[0]保存的使cost value，所以索引要加1
    fg[1 + y_start] = vars[y_start];
    fg[1 + phi_start] = vars[phi_start];
    fg[1 + v_start] = vars[v_start];
    fg[1 + cte_start] = vars[cte_start];
    fg[1 + ephi_start] = vars[ephi_start];
    // 2.2fg的车辆动力学模型约束
    for (size_t t = 1; t < N; ++t) {

        // 2.2.1考虑t时刻的states(the state at time t)
        AD<double> x_0 = vars[x_start + t - 1];
        AD<double> y_0 = vars[y_start + t - 1];
        AD<double> phi_0 = vars[phi_start + t - 1];
        AD<double> v_0 = vars[v_start + t - 1];
        AD<double> cte_0 = vars[cte_start + t - 1];
        AD<double> ephi_0 = vars[ephi_start + t - 1];

        // 2.2.2考虑t+1时刻的states(the state at time t+1)
        AD<double> x_1 = vars[x_start + t];
        AD<double> y_1 = vars[y_start + t];
        AD<double> phi_1 = vars[phi_start + t];
        AD<double> v_1 = vars[v_start + t];
        AD<double> cte_1 = vars[cte_start + t];
        AD<double> ephi_1 = vars[ephi_start + t];

        // 2.2.3对于执行器而言，只考虑t时刻(Only consider the actuation at time t)
        AD<double> delta_0 = vars[delta_start + t - 1];
        AD<double> a_0 = vars[a_start + t - 1];

        AD<double> f_0 = coeffs_[0] + \
                             coeffs_[1] * x_0 + \
                             coeffs_[2] * x_0 * x_0 + \
                             coeffs_[3] * x_0 * x_0 * x_0;    //规划路径的约束方程f(x),将t=0时刻代入

        // phi_desired0为t0时刻，规划路径的切线角.phi_desired0 = d(f_0)/dx
        AD<double> phi_desired0 = CppAD::atan(coeffs_[1] + 2 * coeffs_[2] * x_0 + 3 * coeffs_[3] * x_0 * x_0);

        // 2.3根据车辆动力学模型，添加模型约束。在MPC类中，我们限定了约束的上下限为0，因此，Ipopt求解器将一直求解fg=0的解
        fg[1 + x_start + t] = x_1 - (x_0 + v_0 * CppAD::cos(phi_0) * dt);
        fg[1 + y_start + t] = y_1 - (y_0 + v_0 * CppAD::sin(phi_0) * dt);
        fg[1 + phi_start + t] = phi_1 - (phi_0 + v_0 * delta_0 / Lf * dt);
        fg[1 + v_start + t] = v_1 - (v_0 + a_0 * dt);
        fg[1 + cte_start + t] = cte_1 - (f_0 - y_0 + (v_0 * CppAD::sin(ephi_0) * dt));
        fg[1 + ephi_start + t] = ephi_1 - (phi_0 - phi_desired0 + v_0 * delta_0 / Lf * dt);
    }
}
